from pydobot.dobot import Dobot # 机械臂
import time
# 拍摄图片
from cv_2022 import *
# 文字识别
from ocr.detect_test import detect_prepare
from log.pylog import log
LOGS_INFO = log.info

##################################################################
# 寻找机械臂端口
##################################################################
device = Dobot(port="/dev/dobot_arm", verbose=False)
device._set_queued_cmd_clear()
device.clear_all_alarms_state()
# device.go_home()
device.rotate_to(90, 0, 30, 0, wait=True) # 机械臂初始化归位


'''
description:  从分拣台抓取快递盒，存储在车身上
param {*} catch_coordnate
return {*}
'''
def grab_test(catch_coordnate,save_position):
    # r = device.pose()[3]

    x,y,z = catch_coordnate[0],catch_coordnate[1], catch_coordnate[2]
    # r值取0， 末端执行器不旋转
    device.move_to(x , y , z , 0, wait=True) # 移向抓取目标
    # (x, y, z, r, j1, j2, j3, j4) = device.pose()
    device.suck(True)  # 吸取

    time.sleep(0.6)
    device.move_to(x ,y, z+80 ,0, wait=True) # 抬升
    if save_position == 0:
        device.jump_to(65, 22, 54, 20,  wait=True) # 回到存储区
        # device.rotate_to(65, 22, 54, 20,  wait=True) # 回到存储区
    elif save_position == 1:
        device.jump_to(110, 22, 54, 60,  wait=True) # 回到存储区
        # device.rotate_to(110, 22, 54, 60,  wait=True) # 回到存储区
    # time.sleep(1)
    device.suck(False) # 气泵停止
    time.sleep(0.2)
    if save_position == 0:
        device.rotate_to(-90, 0, 0, 0,wait=True)
    else:
        device.rotate_to(90, 0, 0, 0,wait=True)

    print("grab once Finish")


# 完整单次抓取流程
def grob_route(cnt_centre_list,store_box_num):
    origin_centre = [1080//2, 720 //2 ]
    left_line_x = 450
    right_line_x = 750

    standard_box_centre_pixel = [[365, 450],[630.5, 450.0],[1000, 450.0]]
    standard_catch_points = [[ 106.63, -249.25, -46.25],  #  left
                             [-5.38,  -252.01, -46.08], #  middle
                             [-124.16, -249.06, -46.97]] #  right
    offset_coef_of_Pixel2Real = 3.0
    # 默认抓取左侧
    box_region = 0
    # x y 抓取坐标的补偿值
    x_offset = 0
    y_offset = 0
    for i,[centre,size] in enumerate(cnt_centre_list):
        # 针对多次识别抓取未抓满2个情况，如果当前已经载有一个盒子，则将另一个盒子存在左侧
        LOGS_INFO("store_box_num: {}".format(store_box_num))
        # 每次只抓取2个盒子
        if i> 1:
            break
        if i == 0 and store_box_num > 0:
            store_position  = store_box_num
        else:
            store_position = i
        print(centre)
        # 自适应移动机械臂
        # 确定抓取区域
        if centre[0] < 0 or centre[0] > 1080:
            print("centre coordnate over range 1080")
            continue
        # 左侧
        if centre[0] < left_line_x:
            print("catch LEFT")
            box_region = 0
        # 中间
        elif centre[0] < right_line_x:
            print("catch MIDDLE")
            box_region = 1
        # 右侧
        elif centre[0] >= right_line_x:
            print("catch RIGHT")
            box_region = 2
        else:
            LOGS_INFO("centre over range!")

        catch_point = standard_catch_points[box_region]
        pixel_point = standard_box_centre_pixel[box_region]
        # 坐标弥补值= （ 真实坐标  -  标准坐标 ）  *  弥补系数
        x_offset = (pixel_point[0] - centre[0]) / offset_coef_of_Pixel2Real
        y_offset = (pixel_point[1] - centre[1]) / offset_coef_of_Pixel2Real
        catch_point[0] += x_offset
        catch_point[1] -= y_offset
        # dobot arm value limiter
        if catch_point[0] < -160:
            catch_point[0] = -160
        elif catch_point[0] > 140:
            catch_point[0] = 140
        if catch_point[1] < -290: 
            catch_point[1] = -290
        # print(catch_point[0], catch_point[1] ,catch_point[2])
        # 注意 弥补值 的正负， 对于 x和y 是相反的。
        # x 自左向右增大，与机械臂抓取坐标相同，取正值。
        # y 自上向下增大，与机械臂抓取坐标相反，故取负值
        LOGS_INFO("catch position:{} {}".format(box_region,[catch_point[0], catch_point[1] ,catch_point[2]]))
        grab_test([catch_point[0], catch_point[1] ,catch_point[2]],store_position)
        # store_box_num += 1



def grob_route_offset(arm_position,centre,target_p=1):
    if target_p == 1:
        case_num =  centre[0] / 50
        # 右侧情况
        if case_num > 5:
            offset = (case_num - 10 ) * 15 * (-1)
        else:
            offset = case_num  * 15
        
        arm_position[0] += offset

        # if centre[0] < 150:
        #     arm_position[0] += 80
        # elif centre[0] < 200:
        #     arm_position[0] += 60
        # elif centre[0] < 400:
        #     arm_position[0] += 30
        # elif centre[0]< 650:
        #      arm_position[0] += 10
        # elif centre[0]< 800:
        #     arm_position[0] -= 60
        # else:
        #     arm_position[0] -= 80

    elif target_p  == 0:
        arm_position[1] -= 20

        if centre[0] <700:
            arm_position[0] += 40
        if centre[0] <900:
            arm_position[0] += 20
            
    else:
        # arm_position[1] -= 20
        if centre[0] <700:
            arm_position[0] -= 30
        if centre[0] <900:
            arm_position[0] -= 15
    return arm_position

def grob_route_limit(arm_position,target_p=1):
    if target_p == 1:
        if arm_position[0] < -160:
            arm_position[0] = -160
        elif arm_position[0] > 150:
            arm_position[0] = 150
        if arm_position[1] < -290: 
            arm_position[1] = -290

    elif target_p == 0:
        if arm_position[0] < -160:
            arm_position[0] = -160
        elif arm_position[0] > 220:
            arm_position[0] = 220
        if arm_position[1] < -230: 
            arm_position[1] = -230
    else:
        if arm_position[0] < -160:
            arm_position[0] = -160
        elif arm_position[0] > 160:
            arm_position[0] = 160
        if arm_position[1] < -230: 
            arm_position[1] = -230
    return arm_position


'''
    ** grob 抓取动作 V2.0
'''
def grob_route_new(cnt_centre_list,store_box_num,target_p=1):
    if not (target_p >=0 and target_p <3):
        return
    if len(cnt_centre_list )> 2:
        cnt_centre_list = cnt_centre_list[:2]
    arm_list = [[178.83, -167.78, -38],
            [-119.69,-182.58,-38]]
    origin_centre = [540, 360 ]
    offset_coef = 0.1736 # 每个像素代表的实际距离 mm
    for i,[centre,size] in enumerate(cnt_centre_list):
        print("store_box_num:{}".format(store_box_num))
        print("centre:",centre)

         # 每次只抓取2个盒子
        if i> 1 or (i == 1 and store_box_num == 1 ):
            break
        if i == 0 and store_box_num > 0:
            store_position  = store_box_num
        else:
            store_position = i
        if target_p == 1:
            arm_position = [-9.0675, -207.4406, -38]
        else:
            arm_position = arm_list[target_p]
        
        # arm_position = arm_list[target_p].copy # -90 0 5 0

        # LOGS_INFO("origin_arm_position:{}".format(arm_position))

        offset_x = (centre[0] - origin_centre[0]) * offset_coef  
        offset_y = (centre[1] - origin_centre[1]) * offset_coef  
        arm_position[0] -= offset_x
        # 40 <- RGB摄像头与吸盘x坐标偏差值补偿
        # arm_position[0] += 40
        arm_position[1] += offset_y
        # 60 <- RGB摄像头与吸盘y坐标偏差补偿
        arm_position[1] -= 60
        # LOGS_INFO("arm_position:{}".format(arm_position))
        # arm_position = grob_route_offset(arm_position,centre,target_p)
        # LOGS_INFO("arm_position:{}".format(arm_position))
        # arm_position = grob_route_limit(arm_position,target_p)
        # LOGS_INFO("arm_position:{}".format(arm_position))
        
        # case_num =  centre[0] / 50
        # # 右侧情况
        # if case_num > 5:
        #     offset = (case_num - 10 ) * 15 * (-1)
        # else:
        #     offset = case_num  * 15
        # arm_position[0] += offset
        if target_p == 1:
            if centre[0] < 150:
                arm_position[0] += 80
            elif centre[0] < 200:
                arm_position[0] += 60
            elif centre[0] < 300:
                arm_position[0] += 45
            elif centre[0] < 400:
                arm_position[0] += 30
            elif centre[0]< 650:
                arm_position[0] += 10
            elif centre[0]< 800:
                arm_position[0] -= 20
            else:
                arm_position[0] -= 40

            if arm_position[0] < -160:
                arm_position[0] = -160
            elif arm_position[0] > 150:
                arm_position[0] = 150
            if arm_position[1] < -280: 
                arm_position[1] = -280
        else:
            arm_position = grob_route_offset(arm_position,centre,target_p)
            arm_position = grob_route_limit(arm_position,target_p)
        LOGS_INFO("arm_position :{}".format(arm_position))
        grab_test([arm_position[0], arm_position[1] ,arm_position[2]],store_position)


'''
description: 投放快递盒，从车身将快递盒投入投递盒
param {*} layer 层数,此处0为第二层, 1为第一层
return {*}
'''
# 第二层抓取 8.56 , 223.491 , 5.9    89.89 , 34.47 , 45.81 , 0.0
# 第一层抓取 9 ,223.945 , -31.69    89.21 , 46.46 , 57.81,  0.0
def drob_test(layer):
    r = device.pose()[3]
    #                       Right               Left
    catch_coordnate = [[65, 43, 64, 0],[110, 45, 64, 0]]
    if layer !=0 and layer != 1:
        print("drob arm mode error")
        return
    [j1,j2,j3,j4] = catch_coordnate[layer]
    # 转到抓取区域
    device.jump_to(j1,j2,j3,j4, wait=True)
    # 吸取
    device.suck(True)
    time.sleep(0.5)
    # 抬起转向投递盒
    device.jump_to(-90, 45, 0, 0,  wait=True)
    time.sleep(0.5)
    device.suck(False)
    # device.rotate_to(90, 0, 60, 0,  wait=True)

'''
description:  投递执行函数
param {*} catch_coordnate  车身存储位置
return {*} 
'''
def drob_test2(catch_coordnate,step,layer):
    if step == 0:
        r = device.pose()[3]

        x,y,z = catch_coordnate[0],catch_coordnate[1], catch_coordnate[2]
        device.move_to(x , y , z , 0, wait=True) # 移向抓取目标
        device.suck(True)  # 吸取

        time.sleep(0.4)
        device.move_to(x ,y, z+80 ,0, wait=True) # 抬升
        # time.sleep(0.3)
        device.rotate_to(-90, 45, 0, 0,  wait=True)
    else:
        # time.sleep(0.5)
        device.suck(False) # 气泵停止

        if layer == 0:
            device.rotate_to(90, 0, 5, 0,wait=True)
    print("drob once Finish")


def drob_route(cnt_centre_list):
    cnt_centre_list = cnt_centre_list[:2]
    # standard_box_centre_pixel = [[234.6, 443.9],[644.5, 444.0],[1000, 444.0]]
    # standard_catch_points = [[117.837 , -265.959 ,-46.702],  #  left
    #                          [0.243  ,  -263.562 , -46.317], #  middle
    #                          [-112.238, -262.355,  -46.381]] #  right
    middle_spilt_x = 700
    standard_box_centre_pixel = [[375, 584],[870, 540]]
    standard_catch_points = [[-80.40,238.26, -42.37], #  left
                             [ 92.99, 239.04, -42.25]] #  right
    offset_coef_of_Pixel2Real = 3.2
    # 默认抓取左侧
    box_region = 0
    # x y 抓取坐标的补偿值
    x_offset = 0
    y_offset = 0
    for i,[centre,size] in enumerate(cnt_centre_list):
        # 每次只抓取2个盒子
        if i> 1:
            break
        print(centre)
        # 自适应移动机械臂
        # 确定抓取区域
        if centre[0] < 0 or centre[0] > 1920:
            print("centre coordnate over range 1920")
            continue
        # 左侧
        if centre[0] < middle_spilt_x:
            print("catch LEFT")
            box_region = 0
        # 右侧
        elif centre[0] >= middle_spilt_x:
            print("catch RIGHT")
            box_region = 1
        else:
            print("centre unlimite range!")

        catch_point = standard_catch_points[box_region]
        pixel_point = standard_box_centre_pixel[box_region]
        # 坐标弥补值= （ 真实坐标  -  标准坐标 ）  *  弥补系数
        x_offset = ( centre[0] - pixel_point[0]) / offset_coef_of_Pixel2Real
        y_offset = ( centre[1] - pixel_point[1] ) / 1
        catch_point[0] += x_offset
        catch_point[1] -= y_offset
        if catch_point[0] < -160:
            catch_point[0] = -160
        elif catch_point[0] > 150:
            catch_point[0] = 150
        if catch_point[1] < -290: 
            catch_point[1] = -280
        elif catch_point[1] > 300: 
                catch_point[1] = 280
        print(catch_point[0], catch_point[1] ,catch_point[2])
    
        drob_test2([catch_point[0], catch_point[1] ,catch_point[2]])


'''
description: 决赛使用投放快递盒函数
param {*} cnt_centre_list
param {*} layer
return {*}
'''
def drob_route2(cnt_centre_list,layer):
    # TODO: fix points!
    standard_box_centre_pixel =  [[861, 515],[400, 515]]
    standard_catch_points = [[73.69, 217.54, -41.67], #  right  
                             [ -65.85, 217.93, -41.40]] #  left
    offset_coef_of_Pixel2Real = 3.2
    catch_point = standard_catch_points[layer]
    pixel_point = standard_box_centre_pixel[layer]

    # 确保列表下标未超出
    if cnt_centre_list!= [] and  cnt_centre_list[layer:] !=[] :
        # 检测当前点位与标准点位是否对应，避免左侧的抓取点对应右侧的识别点
        if abs(cnt_centre_list[layer][0][0] - pixel_point[0]) < 250:
            # cnt_centre_list=[ position, area_size ]
            centre = cnt_centre_list[layer][0]
            # 坐标弥补值= （ 真实坐标  -  标准坐标 ） /  弥补系数
            x_offset = (pixel_point[0] - centre[0]) / offset_coef_of_Pixel2Real
            y_offset = (pixel_point[1] - centre[1]) / offset_coef_of_Pixel2Real
            catch_point[0] += x_offset
            catch_point[1] -= y_offset
            if catch_point[0] < -160:
                catch_point[0] = -160
            elif catch_point[0] > 140:
                catch_point[0] = 140
            if catch_point[1] < -290: 
                catch_point[1] = -280
            elif catch_point[1] > 230: 
                catch_point[1] = 230
            LOGS_INFO("Drob catch by Adaptation: {}".format(catch_point))
            print(catch_point[0], catch_point[1] ,catch_point[2])
        else:
            LOGS_INFO("catch_point {} not match pixel_point {}".format(cnt_centre_list[layer][0][0],pixel_point[0]))
            LOGS_INFO("Drob catch by Origin: {}".format(catch_point))
            
    else:
        LOGS_INFO("Drob catch by Origin: {}".format(catch_point))
    drob_test2([catch_point[0], catch_point[1] ,catch_point[2]])


'''
    ** drob 抓取流程动作  V3.0 ** 
'''
def drob_route3(layer,step=0):
    grob_position=[[86.68, 178.83, -34.96],
                   [-71.16, 193.01,-34.20]]
    drob_test2(grob_position[layer],step,layer)

'''
description: 决赛使用 投递后检查车身存储区是否已经清空
param {*} layer  传入当前的投放目标在车身的位置, 左0 右1
return {*}
'''
def deliver_check(layer):
    device.rotate_to(90,0,5,0,wait=True)
    deliver_img = pre_take_pic()
    deliver_img[0:300, 0:pipeline_w] = 0
    img,cnt_centre_list,boxes_list,extra_big_box = detect_prepare(deliver_img,1)
    LOGS_INFO("deliver_check cnt_centre_list:{}".format(cnt_centre_list))
    region = None
    if len(cnt_centre_list) < 1:
        LOGS_INFO("Deliver Check Clear!")
        return
    for [centre,size] in cnt_centre_list:
        if centre[0] > 950:
            region == 1
        else:
            region == 0
        if layer >=0 and layer < 2 and region == layer:
            drob_test(layer)
            LOGS_INFO("deliver repeat {}".format(layer))

